package nxtController;
import image.Calc;

import java.io.IOException;

import lejos.nxt.remote.NXTCommand;
import lejos.pc.comm.NXTComm;
import lejos.pc.comm.NXTCommException;
import lejos.pc.comm.NXTCommFactory;
import lejos.pc.comm.NXTConnector;
import lejos.pc.comm.NXTInfo;
//import lejos.pc.tools.NXJControl;
import static java.lang.Math.*;
//import lejos.robotics.RegulatedMotor;

/**The main class for the robot controller*/
public class ControllerMain {
	static NXTCommand command;
	static Client clientInput;
	public static void main(String[] args) throws NXTCommException, IOException, InterruptedException {
		NXTConnector conn = new NXTConnector();

		NXTInfo[] nxts = conn.search(null, null, NXTCommFactory.BLUETOOTH);

		NXTComm nxtComm = NXTCommFactory.createNXTComm(NXTCommFactory.BLUETOOTH);
		nxtComm.open(nxts[0], NXTComm.LCP);

		command = new NXTCommand(nxtComm);

		clientInput = new Client();
	
		controlLoop();
		
	}
	/**
	 * Controls the robot based on input from the Client.
	 */
	public static void controlLoop() throws InterruptedException {	
		
		float head; //heading.data;
		int sleep;
		float Distance, x, y;
		int Kp,Kl;
		double k,l;
		k=1.439;
		l=4.2*2*3.14/360;
		x= (float) (6*29.3);
		y= (float) (4*29.3);
		
		
		while (true){
			head = clientInput.angle;			
			Kp = (int) abs(head*k);		

			Distance = (float) sqrt(pow(clientInput.xDistance*x,2)+pow(clientInput.yDistance*y,2));

			if (clientInput.xDistance==-1){
				System.out.println("Target Reached");
				sleep = 100;
			}	
			else if (abs(head)<max(3, 20-Distance*0.5)){	
				System.out.println("Distance er: " + Distance + ". Heading er: " + head );
				if ((clientInput.xDistance==-2)){
					//System.err.println("No Path!");
					sleep = 100;
				}
				else {

					Kl= (int) (Distance / l);
					sleep = move(15,Kl,15,Kl,1,1);	
				}
			}
			else if (head < 0){
				System.out.println("Distance er: " + Distance + ". Heading er: " + head );
				sleep = move(15, Kp, -15, Kp, 1, 1);
			}
			else {
				System.out.println("Distance er: " + Distance + ". Heading er: " + head );
				sleep = move(-15, Kp, 15, Kp, 1, 1);

			}
			sleep= sleep + clientInput.delay+1000;
			System.out.println("Tid: " + sleep +"ms - ClientDelay: "+clientInput.delay + "ms");
			Thread.sleep(sleep);
			System.out.println("HeadingTreshold: " + max(3, 20-Distance*0.5));
		}
	}

	private static int move(int speed0, int lim0, int speed1, int lim1, int speed2, int lim2 ) {

		try {
			if (command == null) return 0;
			//if (selectors[0].isSelected())
			command.setOutputState(0, (byte) speed0, 0, 0, 0, 0, lim0);
			//if (selectors[1].isSelected())
			command.setOutputState(2, (byte) speed1, 0, 0, 0, 0, lim1);
			//if (selectors[2].isSelected())
			command.setOutputState(1, (byte) speed2, 0, 0, 0, 0, lim2);
		} catch (IOException ioe) {
			System.err.println("IOException updating control");
		}
		final float t0;
		final float t1;
		final float t2;
		
		if(lim0 == 0)
			t0 = 0;
		else
			t0 = abs(lim0/speed0);
		
		if(lim1 == 0)
			t1 = 0;
		else
			t1 = abs(lim1/speed1);
		
		if(lim2 == 0)
			t2 = 0;
		else 
			t2 = abs(lim2/speed2);
		//System.out.println("Ms: " + 1000+100*Calc.max(t0, t1, t2));
		return (int) (150*Calc.max(t0, t1, t2));

	}
}
	

/*
	private static void move2(int speed0, int lim0, int speed1, int lim1, int speed2, int lim2 ) {

		try {
			if (command == null) return;
			//if (selectors[0].isSelected())
			command.setOutputState(0, (byte) speed0, 0, 0, 0, 0, lim0);
			//if (selectors[1].isSelected())
			command.setOutputState(2, (byte) speed1, 0, 0, 0, 0, lim1);
			//if (selectors[2].isSelected())
			command.setOutputState(1, (byte) speed2, 0, 0, 0, 0, lim2);
		} catch (IOException ioe) {
			System.err.println("IOException updating control");
		}
	}
	
}
*/